package falstad;

import java.awt.Event;

public class Project4Robot extends StandardRobot implements Robot {
		
	public Project4Robot(Maze maze) {
		super(maze);
		this.maze = maze;
	}

	
	/** In one direction */
	private static int senseCost = 1;
	/** Per 90deg turn */
	private static int rotateCost = 3;
	/** Per cell moved */
	protected static int moveCost = 5;
	
	protected boolean hasLeftSensor = true;
	protected boolean hasRightSensor = true;
	protected boolean hasFwdSensor = true;
	protected boolean hasBckSensor = false;
	protected boolean canMoveFwd = true;
	protected boolean canMoveBck = true;
	protected boolean canTurnLeft = true;
	protected boolean canTurnRight = true;
	
	private int[] exitDirection;
	private int exitDistance = Integer.MAX_VALUE;
	
	protected boolean obstacleHit = false;
	
	private int battery = 2500;
	
	
	private Maze maze;
	public int distTravelled = 0;
	
	

	@Override
	public void move(int distance, boolean forward) throws HitObstacleException {
		obstacleHit = false;
		while (distance != 0 && obstacleHit == false && maze.state.getState() == Constants.STATE_PLAY) {
			//printCurrentInfo();
			if (forward) 
			{
				try {
					if (canSeeGoalAhead() && distance == 1 && exitDistance != Integer.MAX_VALUE)
					{
						maze.keyDown(null, Event.UP);
					}
					else
					{
						int [] nextPos = new int[2];
						nextPos[0] = getCurrentPosition()[0] + getCurrentDirection()[0];
						nextPos[1] = getCurrentPosition()[1] + getCurrentDirection()[1];
							if (hasWall(nextPos, getCurrentDirection())) {
								obstacleHit = true;
							}
								maze.keyDown(null,  Event.UP);
							
						}
						//System.out.println(nextPos[0] + " " + nextPos[1]);
				}catch (UnsupportedMethodException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			else { //if backward
				try {
					if (canSeeGoalBehind() && distance == 1 && exitDistance != Integer.MAX_VALUE)
					{
						maze.keyDown(null, Event.DOWN);
					}
					else
					{
						int [] nextPos = new int[2];
						nextPos[0] = getCurrentPosition()[0] + getCurrentDirection()[0];
						nextPos[1] = getCurrentPosition()[1] + getCurrentDirection()[1];
							if (hasWall(nextPos, getCurrentDirection())) {
								obstacleHit = true;
							}
							maze.keyDown(null,  Event.DOWN);
						}
						//System.out.println(nextPos[0] + " " + nextPos[1]);
				}catch (UnsupportedMethodException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
					}
			distTravelled ++;
			battery -= moveCost;
			distance --;	
		}
			

		if (obstacleHit) {
			throw new HitObstacleException(0);
		}
	}


	@Override
	public boolean canSeeGoalAhead() throws UnsupportedMethodException {
		boolean goal = false;
//		try
//		{
			if (exitDirection == getCurrentDirection() && exitDistance != Integer.MAX_VALUE) { //if exit is found
					goal = true;
			}
//		}
//		catch(UnsupportedMethodException e)
//		{
//			throw(e);
//		}
		return goal;
	}

	@Override
	public boolean canSeeGoalBehind() throws UnsupportedMethodException {
		boolean goal = false;
		
		int [] nextDir = getRelativeBehind();
//		try
//		{
			if (exitDistance != Integer.MAX_VALUE && nextDir == exitDirection) 
			{ //if exit is found
					goal = true;
			}
//		}
//		catch(UnsupportedMethodException e)
//		{
//			throw(e);
//		}
		return goal;
	}

	@Override
	public boolean canSeeGoalOnLeft() throws UnsupportedMethodException {
		boolean goal = false;
		
		int [] nextDir = getRelativeLeft();
		if (exitDistance != Integer.MAX_VALUE && nextDir == exitDirection) 
		{ //if exit is found
			goal = true;
		}
		return goal;
	}

	@Override
	public boolean canSeeGoalOnRight() throws UnsupportedMethodException {
		boolean goal = false;
		try
		{
			distanceToObstacleOnRight();
			if (hasRightSensor) {	
				if (exitDistance != Integer.MAX_VALUE) { //if exit is found
					goal = true;
				}
			}
		}
		catch(UnsupportedMethodException e)
		{
			throw(e);
		}
		return goal;
	}

	@Override
	public int distanceToObstacleAhead() throws UnsupportedMethodException {
		
		int distance = 0;
		
		if (hasFwdSensor)
		{
			battery -= senseCost;
			distance =  distToWall(getCurrentDirection());
		}
		else
		{
		try{
			if (hasLeftSensor) {
				rotate(Constants.RIGHT_ROTATE);
				distance = distanceToObstacleOnLeft();
				battery -= senseCost;
				rotate(Constants.LEFT_ROTATE);
			}
			else if (hasRightSensor) {
				battery -= senseCost;
			
				rotate(Constants.LEFT_ROTATE);
			
				distance = distanceToObstacleOnRight();
			
				rotate(Constants.RIGHT_ROTATE);
			}
	
			else if (hasBckSensor) 
			{
				battery -= senseCost;
				rotate(Constants.RIGHT_ROTATE);
				rotate(Constants.RIGHT_ROTATE);
				distance  = distanceToObstacleBehind();
				rotate(Constants.LEFT_ROTATE);
				rotate(Constants.LEFT_ROTATE);
			}
			else
			{
				throw new UnsupportedMethodException(getCurrentDirection());
			}
		}
		catch (UnsupportedArgumentException e) 
		{
			e.printStackTrace();
		}
		}	
		return distance;
	}

	@Override
	public int distanceToObstacleOnLeft() throws UnsupportedMethodException {
		
		int[] nextDir; //new direction that shifts current direction to LEFT
		
		nextDir = getRelativeLeft();
		
		int distance = Integer.MAX_VALUE;
		
		try{
			if (hasLeftSensor) {
				battery -= senseCost;
				
				return (distToWall(nextDir));
//				distance =  distToWall(nextDir);
				
				//System.out.println("distanceToObstacleOnLeft() hasLeftSensor\n distance: " + distance);
			}
			else if(hasFwdSensor)
			{
				rotate(Constants.LEFT_ROTATE);
				distance = distanceToObstacleAhead();
				rotate(Constants.RIGHT_ROTATE);
				battery -= senseCost; 
			}
			else if (hasRightSensor) {
				battery -= senseCost;
			
				rotate(Constants.LEFT_ROTATE);
				rotate(Constants.LEFT_ROTATE);
			
				distance = distanceToObstacleOnRight();
			
				rotate(Constants.RIGHT_ROTATE);
				rotate(Constants.RIGHT_ROTATE);
			}
	
			else if (hasBckSensor) 
			{
				battery -= senseCost;
				rotate(Constants.RIGHT_ROTATE);
				distance  = distanceToObstacleBehind();
				rotate(Constants.LEFT_ROTATE);
			}
			else
			{
				throw new UnsupportedMethodException(nextDir);
			}
		}
		catch (UnsupportedArgumentException e) {
			e.printStackTrace();
		}
		return distance;
	}

	@Override
	public int distanceToObstacleOnRight() throws UnsupportedMethodException {
		int[] nextDir;
		if (getCurrentDirection() == Constants.RIGHT)
		{
			nextDir = Constants.BOTTOM;
		}
		else if (getCurrentDirection() == Constants.TOP)
		{
			nextDir = Constants.RIGHT;
		}
		else if (getCurrentDirection() == Constants.LEFT)
		{
			nextDir = Constants.TOP;
		}
		else if (getCurrentDirection() == Constants.BOTTOM)
		{
			nextDir = Constants.LEFT;
		}
		else
		{
			nextDir = new int[]{0,0};
			System.out.println("Invalid Direction");
		}
		
		int distance = Integer.MAX_VALUE;
		
		if (hasRightSensor) {
			battery -= senseCost;
			return distToWall(nextDir);
		}
		else if(hasFwdSensor)
		{
			try {
				rotate(Constants.RIGHT_ROTATE);
				distance = distanceToObstacleAhead();
				rotate(Constants.LEFT_ROTATE);
				//System.out.println("Current Direction: " + getCurrentDirection()[0] + ", " + getCurrentDirection()[1]);
				battery -= senseCost;
			} 
			catch (UnsupportedArgumentException e) {
				e.printStackTrace();
			}
		}
		else if (hasLeftSensor)
		{
			try {
				rotate(Constants.LEFT_ROTATE);
				rotate(Constants.LEFT_ROTATE);
				distance = distanceToObstacleOnLeft();
				
				rotate(Constants.RIGHT_ROTATE);
				rotate(Constants.RIGHT_ROTATE);
				battery -= senseCost;
			}
			catch (UnsupportedArgumentException e) {
				e.printStackTrace();
			}
		}
		else if (hasBckSensor) 
		{
			battery -= senseCost;
			try {
				rotate(Constants.LEFT_ROTATE);
				distance  = distanceToObstacleBehind();
				rotate(Constants.RIGHT_ROTATE);
			} catch (UnsupportedArgumentException e) {
				e.printStackTrace();
			}
		}
		else
		{
			throw new UnsupportedMethodException(nextDir);
		}
		return distance;
	}

	@Override
	public int distanceToObstacleBehind() throws UnsupportedMethodException {
		int[] nextDir;
		int distance = Integer.MAX_VALUE;
		if (getCurrentDirection() == Constants.RIGHT)
		{
			nextDir = Constants.LEFT;
		}
		else if (getCurrentDirection() == Constants.TOP)
		{
			nextDir = Constants.BOTTOM;
		}
		else if (getCurrentDirection() == Constants.LEFT)
		{
			nextDir = Constants.RIGHT;
		}
		else if (getCurrentDirection() == Constants.BOTTOM)
		{
			nextDir = Constants.TOP;
		}
		else
		{
			nextDir = new int[]{0,0};
		}
		
		//Finding Distance
		if (hasBckSensor) 
		{
			battery -= senseCost;
			distance = distToWall(nextDir);
		}
		else if(hasFwdSensor)
		{
			try {
				rotate(Constants.LEFT_ROTATE);
				rotate(Constants.LEFT_ROTATE);
				
				distance = distanceToObstacleAhead();
				//Rotating Back to original direction
				rotate(Constants.RIGHT_ROTATE);
				rotate(Constants.RIGHT_ROTATE);
				battery -= senseCost;
			} 
			catch (UnsupportedArgumentException e) {
				e.printStackTrace();
			}
		}
		else if (hasLeftSensor)
		{
			try {
				rotate(Constants.LEFT_ROTATE);
				distance = distanceToObstacleOnLeft();
				battery -= senseCost;
			} 
			catch (UnsupportedArgumentException e) {
				e.printStackTrace();
			}
		}
		else
		{
			throw new UnsupportedMethodException(nextDir);
		}
		return distance;
	}
	
	
	/**Inputs: Maze, Current Position of Robot, Direction to sense 
	 * Output: Distance to the nearest wall in that direction
	 * NOTE: DOES NOT DEAL WITH BATTERY COSTS
	 * */
	protected int distToWall(int[] direction){
		int[] curPos = getCurrentPosition();
		int boundaryDistance = getBoundaryDistance(direction);
		
		int distanceToWall = 0;
		while (distanceToWall < boundaryDistance && !hasWall(curPos, direction)) {
			curPos[0] += direction[0];
			curPos[1] += direction[1];
			distanceToWall ++;
			}
		//System.out.println("Direction:  "+direction[0]+", "+ direction[1]);
		//System.out.println("Boundary Distance: " + boundaryDistance );
		if (distanceToWall ==  boundaryDistance && !hasWall(curPos,direction))
		{
			//System.out.println("Exit Values Set");
			exitDistance = distanceToWall;
			exitDirection = direction;
			//This is the exit
		}
		else {
			exitDistance = Integer.MAX_VALUE;
		}
		return distanceToWall;
		
	}
	
	private int [] getRelativeBehind()
	{
		int[] nextDir;
		if (getCurrentDirection() == Constants.RIGHT)
		{
			nextDir = Constants.LEFT;
		}
		else if (getCurrentDirection() == Constants.TOP)
		{
			nextDir = Constants.BOTTOM;
		}
		else if (getCurrentDirection() == Constants.LEFT)
		{
			nextDir = Constants.RIGHT;
		}
		else if (getCurrentDirection() == Constants.BOTTOM)
		{
			nextDir = Constants.TOP;
		}
		else
		{
			nextDir = new int[]{0,0};
		}
		return nextDir;
	}
	
	private int[] getRelativeLeft()
	{
		int[] nextDir;
		if (getCurrentDirection() == Constants.TOP)
		{
			nextDir = Constants.LEFT;
		}
		else if (getCurrentDirection() == Constants.LEFT)
		{
		nextDir = Constants.BOTTOM;
		}
		else if (getCurrentDirection() == Constants.BOTTOM)
		{
			nextDir = Constants.RIGHT;
		}
		else if (getCurrentDirection() == Constants.RIGHT)
		{
			nextDir = Constants.TOP;
		}
		else
		{
			nextDir = new int[]{0,0};
		}
	return nextDir;
	}
	
	protected boolean hasWall(int[] curPos, int[] direction) {
		//System.out.println("direction == Constants.LEFT: " + (direction == Constants.LEFT));
		if (direction == Constants.LEFT) {
			return maze.mazecells.hasWallOnLeft(curPos[0], curPos[1]);
		}
		else if (direction == Constants.RIGHT){
			return maze.mazecells.hasWallOnRight(curPos[0], curPos[1]);
		}
		else if (direction == Constants.TOP) {
			return maze.mazecells.hasWallOnTop(curPos[0], curPos[1]);
		}
		/* Assume other direction must be bottom */
		else  {
			assert(direction == Constants.BOTTOM): "Invalid Direction received (not left/right/top/bottom)";
			return maze.mazecells.hasWallOnBottom(curPos[0], curPos[1]);
		}
	}
/**
 * 
 * @param direction
 * @return
 */
	
	protected int getBoundaryDistance(int[] direction)
	{
		int[] curPos = getCurrentPosition();
		int boundary_distance = 0;
//		assert(direction == Constants.LEFT || direction == Constants.RIGHT || direction == Constants.BOTTOM || direction == Constants.TOP): "Invalid Direction received";
		if (direction == Constants.LEFT) {
			boundary_distance = curPos[0];
		}
		else if (direction == Constants.RIGHT) {
			boundary_distance = maze.getWidth()- 1 - curPos[0];
		}
		else if (direction == Constants.BOTTOM) {
			boundary_distance = maze.getHeight() - 1 -  curPos[1];
		}
		else if (direction == Constants.TOP) {
			boundary_distance = curPos[1];
		}
		else
		{
			System.out.println("Invalid Direction Passed");
		}
		//System.out.println("Direction: " + direction[0] + ", "+ direction[1]+"\nBoundary Distance: "+ boundary_distance );
		return boundary_distance;
	}
	
	public void printCurrentInfo()
	{
		System.out.println("\nDisplaying Current Robot Information\n ================================");
		System.out.println("Current Position: " + getCurrentPosition()[0] + ", "+ getCurrentPosition()[1]);
		System.out.println("Current Direction: " + getCurrentDirection()[0] + ", " + getCurrentDirection()[1]);
//		try {
//			System.out.println("Distance to Forward Wall: " + distanceToObstacleAhead());
//		} catch (UnsupportedMethodException e) {
//			e.printStackTrace();
//		}
//		try {
//			System.out.println("Distance to Left Wall: " + distanceToObstacleOnLeft());
//		} catch (UnsupportedMethodException e) {
//			e.printStackTrace();
//		}
//		try {
//			System.out.println("Distance to Behind Wall: " + distanceToObstacleBehind());
//		} catch (UnsupportedMethodException e) {
//			e.printStackTrace();
//		}
//		try {
//			System.out.println("Distance to Right Wall: " + distanceToObstacleOnRight());
//		} catch (UnsupportedMethodException e) {
//			e.printStackTrace();
//		}
		
	}
	public int getDistTravelled() {
		return distTravelled;
	}
	
	public void escape() {
		maze.keyDown(null, maze.ESCAPE);
	}
	
	public void canSeeGoal(int[] direction)
	{
		
	}
	
	public int getDistTraveled()
	{
		return distTravelled;
	}


	@Override
	public void moveWithRotations(int i, boolean b) {
		// TODO Auto-generated method stub
		
	}
	
	
}
